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Chapter  1 


Introduction 


The  report  documents  the  research  accomplishments  for  the  first  two  years  of  a  three-year 
program  titled  “Large  Space  Manipulators  Study”.  Due  to  budget  constraints,  the  third 
year  of  this  contract  was  not  funded.  This  program  is  sponsored  by  the  Strategic  Defense 
Initiative  Organization  (SDIO)  and  managed  by  the  Air  Force  Office  of  Scientific  Research 
(AFOSR).  This  section  of  the  report  outlines  the  rationale  and  the  specific  objectives  of 
the  research  effort. 


1.1  Rationale  for  the  Research 


Design  of  algorithms  for  rapid  large  angle  slew  maneuvers  and  vibration  supression 
of  space  structures  modelled  as  articulated  chains  of  elastic  bodies  is  a  key  technology 
issue  for  the  SDIO  mission.  Example  of  such  structures  are  (1)  large  space  manipulators 
and  (2)  space-based  lasers  (SBL).  In  the  first  case,  manipulators  such  as  the  Remote 
Manipulator  System  (RMS)  are  characterized  by  two  main  long  links  which  behave  as 
slender  elastic  beams  with  significant  bending  and  torsional  compliance.  The  fundamental 
vibration  frequency  of  the  RMS  is  very  low,  typically  in  the  range  of  0.05  to  0.5  Hz 
depending  on  the  mass  of  the  payload.  It  is  expected  that  long-reach  manipulators,  used  in 
concert  with  small  dexterous  manipulators,  will  play  a  key  role  for  the  satellite  servicing 
activities  related  to  the  SDI  mission.  In  the  later  case,  SBL  are  typically  modelled 
as  truss-like  elastic  structures  hinged  to  large  rigid  platforms.  Standard  linear  models 
derived  from  finite-element  codes  used  by  the  Large  Space  Structures  (LSS)  control 
community  are  not  adequate  to  predict  the  dynamic  behavior  of  this  class  of  elastic 
structures.  Dynamic  models  which  account  for  the  geometric  nonlinearities  due  to  large 
angle  motions  occuring  at  the  articulations  need  to  be  developed  Control  techniques  for 
the  slewing  and  pointing  of  such  structures  can  then  be  studied  based  on  these  nonlinear 
dynamic  models.  A  sequence  of  control  laws  need  to  be  developed  from  simple  linear 
feedback  laws  using  colocated  sensor/actuator  pairs  to  nonlinear  controllers  compensating 
for  the  nonlinear  coupled  dynamics. 
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Figure  1-1:  Experimental  2D  articulated  structure 


1.2  Research  Objectives 


The  research  performed  under  this  program  is  a  combination  of  theoretical  and  exper¬ 
imental  investigations.  Central  to  this  research  is  an  experimental  testbed  for  a  planar  2D 
articulated  elastic  structure.  The  testbed  shown  in  Figure  1  has  a  fundamental  vibration 
frequency  in  the  range  of  0.5  Hz  to  1.5  Hz,  depending  on  the  particular  links/payload 
configuration.  The  testbed  is  used  both  for  dynamic  model  validation  as  well  as  for 
demonstration  of  various  control  strategies. 

The  specific  research  objectives  for  the  original  three  year  program  as  outlined  in 
the  statement  of  work  are  given  below: 


•  Develop  a  detailed  dynamic  mathematical  model  for  a  representative  three-dimensional 
elastic,  articulated  structure. 

•  Design  and  implement  a  series  of  compensators  for  a  laboratory  model  of  an  artic¬ 
ulated  elastic  structure  moving  in  the  horizontal  plane. 

•  Develop  concepts  for  a  nonlinear  control  law  applied  to  the  elastic  planar  manip¬ 
ulator  testbed. 

•  Study  the  feasability  of  implementing  advanced  control  concepts  developed  by 
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other  investigators  on  the  experimental  testbed. 

•  Extend  advanced  control  concepts  to  fully  three-dimensional  space  structure  im¬ 
plementations. 


The  next  two  sections  of  the  report  documents  the  progress  made  in  accomplishing 
the  research  objectives  during  the  first  two  years  of  the  contract.  Appendix  A  lists 
the  professional  personnel  associated  with  the  research  and  the  interactions  (coupling 
activities)  performed  in  the  context  of  this  contract. 


Chapter  2 


Dynamic  Models  of  Articulated  Elastic 

Structures 


2.1  Introduction 


The  goal  of  this  task  is  (1)  to  derive  equations  of  motion  in  closed  form  for  articulated 
chains  of  elastic  bodies  (2-D  and  3-D)  and  (2)  to  validate  the  corresponding  dynamic 
models  against  experimental  data  for  the  case  of  a  planar  testbed. 

The  generic  articulated  structure  studied  here  is  shown  in  Figure  2-1.  The  first  body 
(shoulder  joint)  is  rigid  and  the  two  main  links  are  modelled  as  slender  elastic  beams 
which  can  bend  in  two  orthogonal  directions.  A  rigid  “payload”  is  cantilevered  at  the  tip 
of  the  second  link.  The  three  joint  angles  (9},  02>  63)  can  take  arbitrarily  large  values. 
Three  torque  actuators  are  located  at  the  joints.  A  2-D  version  of  the  test  structure  is 
obtained  by  locking  the  first  joint  0X\  in  this  case,  the  plane  of  motion  is  orthogonal  to 
the  joint  axes  2  and  3  (only  the  elastic  deformations  in  that  plane  are  considered).  The 
experimental  testbed  shown  in  Figure  2-2  has  been  designed  to  emulate  the  2-D  planar 
version  of  the  articulated  structure. 

In  Section  2.2  we  review  the  general  method  of  derivation  based  on  Kane’s  equations. 
The  simplified  nonlinear  mass  matrix  and  stiffness  matrix  for  the  3-D  articulated  struc¬ 
ture  are  described  in  Appendix  B.  In  Section  2.3  we  briefly  describe  the  three  dynamic 
simulation  models  coded  for  the  planar  elastic  structure.  We  also  discuss  the  selection 
of  the  assumed  mode  shapes.  Finally  we  document  the  experimental  validations  of  the 
linearized  and  the  nonlinear  analytical  models. 


2.2  General  Method  of  Derivation 


The  equations  of  motion  for  the  elastic  articulated  structure  described  above  are 
obtained  using  Kane’s  method.  The  first  step  in  the  derivation  is  to  replace  the  distributed 
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Figure  2-1:  Schematic  of  the  3-D  flexible  articulated  structure:  the  two  main  links  are 
modelled  as  slender  elastic  beams  with  uniform  mass  properties.  Lumped  masses  at  the 
tip  of  the  inner  and  outer  beams  represent  the  elbow  joint  drive  and  the  tip  payload 
respectively. 


Figure  2-2:  Photograph  of  the  Experimental  Flexible  Manipulator  Cshown  here  in  a  rigid- 
flex  configuration) 
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elastic  coordinates  for  each  elastic  body  by  a  finite  series  of  the  form 

v(x,t)  =  £$j(x)qi(t)  (2-1) 

i=l 

where  the  “assumed”  mode  shapes  $;(x)  form  a  set  of  admissible  functions  satisfying  the 
geometric  boundary  conditions  for  the  elastic  deflections.  The  issue  of  the  selection  of  the 
assumed  mode  shapes  is  discussed  in  the  next  section.  The  time-dependent  coordinates 
qi(t )  are  now  new  generalized  coordinates  for  the  problem.  The  state  vector  X  is  then 
defined  as  [XT,Xei]  where  Xr  consists  of  the  3  rigid-body  coordinates  0,(t)  and  Xtl 
consists  of  nt  elastic  coordinates  g,(f). 

Using  any  derivation  method,  Lagrange,  Newton  or  Kane’s,  the  equation,  of  motion 
for  the  elastic  structure  will  be  of  the  form: 


M(X)X  +  DX  +  KX  =  GU  +  T„iin(X,X)  (2  -  2) 

where  M(X)  is  the  configuration-dependent  mass  matrix,  D  and  I\  are  respectively  the 
damping  and  the  stiffness  matrices  for  the  composite  elastic  structure,  Tnij„(X,X)  are 
the  nonlinear  torques  arising  from  the  centrifugal  and  Coriolis  forces,  G  is  the  control  dis¬ 
tribution  matrix  and  U  is  a  3  by  1  vector  of  actuator  torques.  This  formulation  implicitely 
assumes  that  linear  elasticity  theory  is  used  to  describe  the  structural  deformations  of  the 
elastic  bodies;  consequently,  the  terms  of  order  which  may  appear  in  course  of  the 
derivation  of  equation  (2-2)  should  not  be  included.  As  we  discussed  already  in  [2],  the 
mass  and  stiffness  matrix  can  be  expressed  as: 


M{  X)  =  M(Xr)  +  M(Xr,Xel)  (2-3) 

Tniin(X,X)  =  Tnltn(Xr,Xr)  +  TnU„(X,X)  (2-4) 

where  the  terms  with  the  superscript  -  depend  only  on  the  rigid  coordinates  ,Yr  while 
the  ones  with  the  superscript  *  are  linear  in  the  elastic  coordinates  Xet  and/or  in  the 
time-derivatives  Xel. 

A  simplified  dynamic  model  is  obtained  by  evaluating  the  terms  dependent  only  on 
the  rigid  coordinates, i.e.,  the  mass  matrix  M(XT)  and  the  nonlinear  torques  Tn|,n(.Yr,  A\) 
which  are  both  zeroth-order  in  Xe\.  Such  a  model  is  much  easier  to  derive  than  the 
complete  model.  As  discussed  in  the  next  section,  comparisons  with  the  experimental 
data  obtained  with  the  planar  testbed  have  shown  (so  far)  that  the  simplified  model  is 
sufficient  to  accurately  predict  the  rigid/elastic  dynamic  effects. 

There  arc  no  real  theoretical  difficulties  in  deriving  analytical  expressions  for  each 
term  of  ( 2-2).  The  matrices  K,  D  and  G  are  readily  obtained  as  constant  matrices  derived 
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from  the  assumed  mode  shapes  $,(i)  selected  for  each  elastic  body.  The  main  challenge 
is  in  the  time-consuming,  error-prone  derivations  required  to  compute  the  elements  of  the 
mass  matrix  M( X)  and  of  the  vector  of  nonlinear  torques  Tniin.  In  contrast  to  Lagrange’s 
method,  Kane’s  method  enables  the  analyst  to  compute  each  of  these  terms  independently 
and  using  only  simple  vectorial  operations.  If  M,-  is  a  generic  point  along  link  i,  the  linear 
velocity  Vm,  and  acceleration  Am ,  of  M,  can  be  expressed  as: 


VMi  =  Gm;X  (2-5) 

AM;  =  GMiX  +  XT— -IX  (2-6) 

aGM. 

The  row  vector  Gm;  of  X  partial  velocities  Vj^.  and  the  N  x  N  matrix  H  =  --y  1  are 
obtained  solely  from  expressions  based  on  elementary  differential  geometry. 


Having  derived  the  partial  velocities  vector  VrM.,  the  mass  matrix  M(X)  is  then 
simply  obtained  as: 

">..  =  £  jB  V^  V^.dm  (2-7) 


Conversely,  the  nonlinear  torques  Tnijn(X,X)  are  expressed  as: 


TLin(X,X)  =  £  X" 


/B.  HM|  •  V^.dm 


(2-8) 


where  the  dot  product  involves  each  vector  element  of  the  matrix  Hm;  with  V^j 


These  calculations  involving  only  dot  products  and  partial  differentiation  of  vectors 
are  much  easier  to  perform  than  the  lengthy  derivations  of  the  Lagrange  method.  In 
Appendix  B  we  illustrate  the  main  steps  in  the  derivation  of  the  simplified  mass  matrix 
M(Xr)  and  of  the  stiffness  matrix  K  for  the  3-D  elastic  structure  described  above. 


2.3  Dynamic  Models  for  the  2-D  Elastic  Articulated  Struc¬ 
ture 


Figure  2-3  shows  a  schematic  of  the  planar  structure.  Each  link  is  modelled  as  an 
Euler-Bemouilli  slender  elastic  beam  with  rigid  inertias  at  each  end.  The  testbed  has  been 
used  in  three  different  configurations:  (1)  single  elastic  link,  (2)  2-DOF  rigid-flex  arm 
where  the  inner  link  is  rigid  and  (3)  2-DOF  flex-flex  arm  where  both  links  are  flexible. 
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Figure  2-3:  Schematic  of  planar  elastic  arm 

Accordingly  three  different  simulations  have  been  developed  for  model  validation  and 
control  designs.  All  the  simulations  have  been  coded  using  the  System-Build  simulation 
tool  of  the  MATRIXx  CAD  software  [14].  The  MATRIXx  software  provides  a  convenient 
environment  for  control  design,  simulation  and  comparison  with  the  experimental  data. 

The  first  simulation  is  a  single  elastic  link  model.  In  this  case,  the  rigid/elastic  dy¬ 
namics  are  written  in  modal  form.  The  coefficients  of  the  state-space  model  (frequencies 
and  mode  shapes)  are  obtained  from  a  user-defined  function  in  MATLAB  [13];  this  code 
solves  for  the  (exact)  system  frequencies  and  mode  shapes  of  a  composite  elastic  beam 
pinned  at  one  end  and  loaded  by  a  rigid  inertia  at  the  other  end.  The  linear  state-space 
model  used  for  control  design  and  validation  is  typically  8th-order  with  one  rigid-body 
mode  and  the  first  three  elastic  modes.  The  simulation  includes  also  a  general  analog  or 
discrete  linear  compensator  model,  a  simple  nonlinear  model  for  the  Coulomb  friction 
and  simple  filters  to  represent  the  sensors  dynamics  as  shown  in  the  block  diagram  of 
Fig.  2-4. 

The  second  simulation  is  a  nonlinear  dynamic  model  of  the  2-D  structure  with  an 
inner  rigid  link  and  an  outer  elastic  link.  A  complete  model  including  the  foreshort¬ 
ening  effects  due  to  kinematic  nonlinearities  has  been  described  in  [2].  A  simplified 
model.where  all  the  terms  involving  the  elastic  coordinates  in  the  mass  matrix  and  in  the 
nonlinear  torques  are  eliminated,  has  also  been  developed. 

Finally  the  third  simulation  is  a  nonlinear  dynamic  model  of  the  2-D  structure  with 
both  links  flexible.  The  code  was  developed  by  C.  Padilla  and  is  documented  in  [5].  The 
user  can  select  either  the  complete  model  including  all  the  first-order  terms  dependent  on 
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CouJomb  Friction 


Figure  2-4:  Simulation  Block  Diagram  for  the  Single  Elastic  Link  Linear  Dynamic  Model 


the  elastic  coordinates,  or  the  simplified  model  where  these  terms  are  neglected.  As  done 
for  the  single  link  simulation,  actuator  and  sensor  dynamic  models  are  included  together 
with  the  compensator  model  (discrete  or  analog)  as  shown  in  Fig.  2-5. 


2.3.1  Choice  of  Mode  Shapes  for  2-D  Dynamic  Model 

We  discuss  here  the  issue  of  mode  shapes  selection  for  the  arm  in  the  flex-flex  configu¬ 
ration.  The  assumed  mode  shapes  chosen  for  each  link  are  the  cantilevered  modes  <f>c,{x) 
of  a  uniform  elastic  beam  with  a  rigid  tip  body.  Using  the  mode  shapes  of  a  loaded 
beam  allows  for  transmission  cf  shear  force  and  bending  moment  at  the  elbow  joint. 
In  the  finite-element  litterature,  these  modes  are  sometimes  referred  as  “loaded-interface 
modes”.  As  shown  in  Figure  2-3,  the  cantilevered  boundary  conditions  are  compatible 
with  the  particular  choice  of  the  frame  of  reference  in  which  the  elastic  deflections  are 
defined.  For  the  inner  link,  it  is  not  clear  what  mass  properties  to  use  for  the  rigid  tip 
body.  One  possible  choice  would  be  the  combined  elbow  drive,  outer  link  (rigidized) 
and  tip  payload,  with  the  elbow  joint  locked  at  some  arbitrary  elbow  angle.  For  the  outer 
link,  the  natural  choice  is  to  use  the  actual  tip  payload  as  the  equivalent  tip  body. 

Given  a  particular  set  of  assumed  mode  shapes  #(x)  for  each  link,  the  generalized 
eigenvalue  problem  describing  the  small  vibrations  of  the  2-D  elastic  arm  around  a  given 
configuration  is  given  as: 
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Figure  2-5:  Simulation  Block  Diagram  for  the  2-D  Nonlinear  Elastic  Dynamic  Model 


(M(6%)s2  +  Ds  +  k)X(s)  =  0  (2-9) 

where  Of  is  the  reference  elbow  joint  angle. 

For  each  particular  choice  of  the  assumed  modes,  one  can  derive  the  system  vibra¬ 
tion  frequencies  Qp  and  the  system  mode  shapes  $p(i)  by  solving  for  (2-9).  Detailed 
numerical  studies  have  shown  that  accurate  prediction  of  these  frequencies  and  mode 
shapes  is  not  sensitive  to  the  particular  choice  of  the  mass  properties  of  the  equivalent 
rigid  body  for  the  inner  link.  Figure  2-6  shows  transfer  functions  from  shoulder  motor 
torque  to  shoulder  motor  velocity  for  two  cases.  Three  modes  are  used  for  each  elastic 
link  and  the  elbow  joint  is  unlocked.  The  solid  line  results  from  computing  the  inner 
link  mode  shapes  using  the  nominal  equivalent  rigid  body  payload  mass  properties  mf 
and  If  (0E  =  0°);  this  corresponds  to  the  maximum  value  for  payload  mass  and  inertia 
at  the  shoulder  tip.  For  the  dashed  line  of  Fig.  2-6,  the  equivalent  payload  is  chosen  to 
include  only  the  elbow  actuator  stator,  compared  to  the  nominal  case,  this  represents  a 
reduction  of  three  in  trip  and  two  orders  of  magnitude  in  If.  Despite  the  gross  differences 
in  the  mass  properties  chosen  for  the  inner  link  payload,  very  little  difference  is  seen 
between  the  two  plots;  the  poles  (system  mode  frequencies)  and  the  zeroes  of  the  transfer 
function  remain  essentially  invariant.  The  system  mode  shapes  (not  shown  here)  are  also 
essentially  the  same. 

Finally,  we  consider  the  system  modes  for  a  future  version  of  the  experimental  arm 
having  a  12-ft  reach,  twice  that  of  the  current  configuration.  In  this  case,  elastic  links  of 
the  same  bending  stiffness  are  used,  and  their  lengths  chosen  to  give  6  feet  between  joint- 
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Figure  2-6:  Open-loop  transfer  function  from  shoulder  torque  to  shoulder  angle  obtained 
for  two  different  tip  body  mass  properties  of  the  inner  elastic  link  (6-ft  arm  in  extended 
configuration) 

axes.  Figure  2-7a.  shows  the  transfer  function  from  shoulder  motor  torque  to  shoulder 
motor  velocity,  while  Fig.  2-7b.  is  from  shoulder  joint  torque  to  output  from  a  strain 
gauge  mounted  midway  along  the  shoulder  link.  As  with  Fig.  2-6,  the  solid  lines  are 
obtained  using  the  nominal  m £  and  Ip(0E  =  0°)  while  the  dashed  lines  use  mode  shapes 
of  the  inner  link  based  on  the  equivalent  payload  mass  properties  of  the  elbow  actuator 
stator  alone.  As  can  be  seen,  again  the  system  frequencies  are  insensitive  to  the  particular 
choice  of  the  mass  properties  of  the  equivalent  rigid-body  payload. 


2.3.2  Experimental  Validation  of  the  Linearized  Models 

In  [2]  we  discussed  the  identification  of  the  dynamic  model  for  a  single  elastic  link. 
We  now  compare  the  experimental  versus  analytical  transfer  functions  for  the  2-DOF 
manipulator  in  the  flex-flex  configuration.  Figure  2-8  shows  the  transfer  functions  from 
shoulder  motor  torque  to  shoulder  motor  velocity  for  two  nominal  values  of  the  elbow 
angle  0E.  During  the  frequency  response  tests,  the  elbow  joint  is  left  unlocked.  The 
nominal  elbow  angles  are  0°  and  90°  for  Figs.  2-8. a  and  2-8.b,  respectively.  The 
analytical  results  are  based  on  a  linearization  of  the  model  about  nominal  elbow  angle 
and  with  3  modes  retained  for  each  link.  The  natural  vibrations  of  the  2-DOF  system 
are  predicted  by  Eq.  (2-9).  For  each  value  of  0E,  the  theoretical  model  matches  the 
first  two  poles  and  zeros  quite  well,  and  the  only  significant  difference  is  seen  in  the 
third  mode  at  approximately  85  Hz.  Note  that  for  the  experimental  results,  the  flexible 
modes  appear  much  more  damped  than  shown  in  the  model.  This  apparent  structural 
damping  is  largely  due  to  the  significant  amount  of  viscous  and  coulomb  damping  in  the 
gearmotor,  and  the  relatively  coarse  resolution  obtained  in  the  measurements.  For  the 
model,  structural  damping  was  assumed  to  be  approximately  1.5%  in  each  of  the  modes. 
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Figure  2-7:  Open-loop  transfer  function  (a)  from  shoulder  torque  to  shoulder  angle  and 
(b)  from  shoulder  torque  to  mid-span  strain  gauge  obtained  for  two  different  tip  body  mass 
properties  of  the  inner  elastic  link  (12-ft  arm  in  extended  configuration ) 


Figure  2-8:  Experimental  versus  Analytical  Transfer  Function  from  Shoulder  Motor 
Torque  to  Shoulder  Motor  Velocity  for  an  elbow  angle  equal  to  (a)  0°  and  (b)  90° 
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Table  2.1:  Analytical  Clamped- Free  (fi,-)  and  Free-Free  (u>,)  Frequencies  for  the  2-DOF 
arm  with  a  large  tip  payload 

Table  2.1  summarizes  the  free-free  (i.e.,  both  joints  free)  and  clamped-free  (i.e.,  both 
joints  locked)  frequencies  of  the  flex-flex  manipulator  for  elbow  angles  of  0°  and  90°. 
As  seen  in  the  table,  the  free-free  frequencies  u>,  are  insensitve  to  the  elbow  angle  as  the 
u do  not  vary  by  more  than  2%;  however,  the  first  bending  frequencies  ft,  vary  as  much 
as  15%. 


233  Experimental  Validation  of  the  Nonlinear  Dynamic  Models 

The  nonlinear  dynamic  model  is  validated  by  comparing  simulated  and  experimental 
time  histories  of  given  sensors  for  large  angle  motion  of  the  structure.  Figure  2-9 
shows  time  histories  of  the  experimental  and  simulated  joint  angles,  velocities  and  strain- 
gauges  output  for  a  slew  maneuver  of  the  arm  in  the  rigid-flex  configuration  [2].  Fifth- 
order  splines  are  used  as  the  joint  angle  reference  commands  for  an  independent  joint 
Proportional  and  Derivative  controller.  The  PD  control  commands  are  shaped  with  digital 
lowpass  filters  to  provide  gain  stabilization  of  the  higher  modes.  Note  that  here,  the 
simplified  dynamic  model  (for  which  the  terms  in  the  mass  matrix  and  the  nonlinear 
torques  dependent  on  the  elastic  coordinates  are  deleted)  is  used  in  the  simulation.  Figure 
2-10  shows  time  histories  for  a  similar  maneuver  with  the  arm  in  a  flex-flex  configuration. 
The  top  two  plots  of  Figure  2-10  show  the  joint  angle  responses  for  a  20°  step  command 
on  each  joint.  These  responses  are  dominated  by  the  rigid  body  dynamics.  To  highlight 
the  elastic  dynamics,  the  bottom  plot  compares  the  experimental  and  simulated  response 
of  voltage  output  from  a  strain  gauge  sensor  mounted  at  about  mid-span  on  the  elbow 
link.  In  each  plot,  good  agreement  is  seen  between  model  and  experiment. 
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Figure  2-9:  Experimental  and  Simulated  Step  Responses  using  PD  Control  for  arm  in 
rigid-flex  configuration 
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Figure  2-10:  Experimental  and  Simulated  Step  Responses  using  PD  Control  for  arm  in 
flex-flex  configuration 
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2.4  Conclusion 


The  derivation  of  analytical  dynamic  models  for  articulated  elastic  structures  has 
been  discussed.  Kane’s  method  allows  for  straightforward  derivation  of  the  equations  of 
motion.  The  validation  of  the  2-D  dynamic  model  has  also  been  successfully  obtained. 
From  the  tests  performed  so  far  on  the  experimental  structure,  it  seems  that  the  simplest 
dynamic  model  for  which  the  first-order  terms  in  the  elastic  coordinates  are  neglected 
in  the  mass  matrix  and  nonlinear  torque  expressions  accurately  predicts  the  rigid/elastic 
coupling  effects.  Design  and  implementation  of  control  designs  for  the  2-D  structure  are 
discussed  in  the  next  Chapter. 


2-13 


Chapter  3 


Control  Law  Designs  and 
Implementations 


In  this  section,  linear  control  designs  based  on  the  Linear  Quadratic  Gaussian  with  Loop 
Transfer  Recovery  (LQG/LTR)  theory  are  discussed  first  for  a  single  elastic  link  and 
then  for  the  2-DOF  elastic  arm  in  the  rigid-flex  and  flex-flex  configurations.  Appendix 
C  provides  a  partial  listing  of  C-code  subroutines  used  to  implement  (on  the  real-time 
control  system)  a  sample  control  law  for  the  2DOF  manipulator. 


3.1  Single  Link  Active  Modal  Control 


For  these  experiments,  the  outer  elastic  link  of  the  manipulator  is  used  with  the  elbow 
joint  effectively  grounded  (by  turning  off  air  supply  to  the  elbow  air  bearing).  During  the 
first  phase  of  this  contract  [2],  we  demonstrated  a  high-performance  LQG  controller  for 
the  outer  link  configured  with  a  small  payload  of  mass  mv  =  lAkg.  Below,  we  address 
the  more  challenging  problem  of  designing  a  high-performance  active  modal  controller 
for  the  case  of  a  much  larger  payload  (mp  =  4.4 kg).  While  we  have  have  designed 
and  implemented  several  controllers  using  various  sets  sets  of  sensors,  the  best  results 
were  obtained  with  an  LQG  design  using  joint  position  and  velocity  sensors  along  with  a 
strain  gauge  mounted  to  the  link.  After  presenting  this  design  and  experimental  results, 
we  then  examine  the  controller’s  sensitivity  to  changes  in  payload  moment-of-inertia  and 
one  method  we  have  used  to  yield  a  more  robust  compensator  design. 


Control  Design  Issues  for  Heavy  Payloads 

For  large  payloads,  a  high-performance  controller  is  more  difficult  to  achieve  for  two 
reasons.  Foremost  is  the  fact  that  the  system’s  first  cantilevered  bending  frequency  fic 
decreases  with  larger  payloads.  In  our  case,  flc  decreases  by  a  factor  of  two  (from  1.2 Hz 
to  0.6 Hz  for  the  small  and  large  payloads,  respectivley).  When  using  a  colocated  con¬ 
trol  scheme,  acceptably  damped  closed-loop  position  bandwidth  has  been  shown  to  be 
fundamentally  limited  to  0.5ftc.  For  the  small  payload  case,  this  fundamental  limit  was 
overcome  with  a  LQG  regulator  and  estimator  design  incorporating  tip  velocity  measure- 
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Figure  3-1:  Experimental  transfer  function  from  elbow  torque  to  voltage  output  of  a  strain 
guage  mounted  near  the  midpoint  of  the  elbow  link.  Intended  to  primarily  sense  the  first 
bending  mode,  the  strain  gauge  was  located  near  a  strain  anti-node  of  the  first  mode. 
As  the  frequency  response  illustrates,  this  results  in  a  near  pole-zero  cancellation  of  the 
second  mode  and  provides  35 dB  attenuation  compared  to  the  first  mode. 

ments.  With  this  non-colocated  controller,  closed-loop  bandwiths  on  the  order  of  0.9f2c 
were  achieved.  However,  as  discussed  in  [2],  increasingly  large  payloads  effectively 
“pin”  the  tip.  As  a  result,  the  elastic  bending  modes  become  poorly  observable  with  a  tip 
sensor,  and  thus,  tip  sensors  are  not  effective  for  active  modal  damping  of  the  dominant 
bending  modes.  To  remedy  this  problem,  we  have  made  use  of  strain  gauge  sensors 
mounted  along  the  link(s).  As  discussed  in  [2],  the  strain  gauge  location  was  optimized 
to  maximize  the  observability  of  the  dominant  bending  mode  (see  Fig.  3-1).  The  results 
presented  below  demonstrate  the  active  modal  damping  improvements  achieved  with  the 
use  of  strain  sensors,  but  first  we  breifly  review  the  LQG/LTR  design  method  used. 


3.1.1  LQG/LTR  Controller  Design  Method 

As  noted  above,  the  elbow  joint  is  assumed  grounded;  thus,  a  separate  model  expressed  in 
modal  form  (derived  previously  in  [2])  is  used  for  the  outer  elastic  link.  For  the  control 
design,  only  the  first  two  flexible  modes  are  retained,  yielding  a  6th-order  state-space 
model.  Using  the  Separation  Theorem,  the  controller  design  is  done  in  two  steps:  1) 
design  of  a  full-state  feedback  regulator,  and  2)  design  of  a  steady-state  estimator.  The 
resulting  compensator  is  then  analyzed  in  terms  of  robustness  for  the  resulting  closed- 
loop  system.  When  necessary,  frequency-shaped  weightings  are  used  in  the  regulator’s 
performance  index  to  provide  gain  stabilization  of  the  uncontrolled  higher  frequency 
modes.  (Frequency  shaping  of  the  performance  index  is  discussed  later  in  section  3.3.) 
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Regulator  Design 


The  regulator  is  designed  to  optimize  a  given  quadratic  performance  index  of  the  follow¬ 
ing  form: 


/+oo 

(y‘Qy  +  pu2e)dt  (3-1) 

•oo 

where  y  =  Cyx  is  a  vector  of  user-selected  plant  outputs,  Q  is  a  diagonal  weighting 
matrix  on  y,  and  p  is  a  weighting  on  the  control  torque  ue.  The  paramater  p  is  used  as 
a  “tuning  knob”  to  adjust  the  rigid-body  bandwidth,  based  on  the  available  peak  torque 
from  the  actuator.  The  regulator  closed-loop  poles  are  the  stable  roots  of  the  following 
characteristic  equation: 

det\pl  +  Ht(-s)QH(+s)]  =0  (3-2) 


where  H(s)  is  the  transfer  function  matrix  from  u(s)  to  y(s).  In  order  to  help  in  the 
choice  of  outputs  y  and  the  diagonal  matrix  Q,  we  use  a  general  property  of  the  optimal 
regulator  root-locus  derived  in  [8].  When  the  control  weighting  p  tends  to  zero,  (“cheap 
control”),  p  of  the  regulator  poles  are  obtained  as  the  stable  transmission  zeros  of  the 
following  system  of  order  2 n: 

A  0 

Ct,QC,  -At 

K°l 

[  0  -  BT  ]  (3-3) 

where  A  and  B  are  the  state-space  and  control  distibution  matrices,  respectively.  Studying 
the  location  of  these  finite  zeros  for  a  given  Jy  will  guide  the  designer  in  choosing  the 
set  of  outputs  y  and  the  elements  of  the  Q  matrix. 

Figure  3-2  shows  the  optimal  regulator  root  locus  as  p  is  varied.  1  In  this  figure, 
the  open  loop  poles  are  marked  by  x’s,  while  the  asymptotic  regulator  pole  locations 
(for  p  =  0)  are  marked  by  circles.  In  this  design  for  the  large  payload,  the  outputs  of 
y  were  initially  chosen  to  be  the  joint  angle  and  joint  velocity.  In  this  case,  the  finite 
zeros  on  the  ju>  axis  would  correspond  to  the  the  open-loop  transmission  zeros  of  the 
system  (A,  B,  Cv)  (i.e.,  the  cantilevered  bending  frequencies  of  the  arm).  For  the  chosen 
form  of  the  system  model,  the  measurement  vector  Cy  is  a  function  of  the  modal  slopes 
at  the  hub;  by  directly  weighting  the  modal  slopes  of  Cy  corresponding  to  the  first  two 

‘see  Appendix  C  fora  listing  of  the  user-defined  MATRJXx  functions  developed  to  generate  the  optimal 
regulator  and  estimator  root-loci  shown  in  this  chapter. 
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Figure  3-2:  Optimal  regulator  root  locus  versus  control  weighting  p  for  LQR  design  of 
outer  elastic  link  with  a  large  payload. 

elastic  modes,  the  finite  zeros  are  shifted  upward  to  the  more  desirable  locations  shown 
in  Fig.  3-2.  The  chosen  closed-loop  regulator  poles  are  marked  in  the  figure  by  a  star. 
For  this  value  of  p,  all  of  the  modes  are  well-damped,  and  the  rigid-body  poles  give  a 
bandwidth  roughly  equal  to  0.85nc. 


Estimator  Design 

The  Loop  Transfer  Recovery  method  applied  here  in  its  simplest  form  [7]  is  used  to  select 
the  process  noise  intensity  q  (modelled  here  as  actuator  noise)  for  the  estimator  design. 
Central  to  this  method  is  the  proper  choice  of  the  ratios  r,  =  where  Rvr,m  is  the 

noise  intensity  for  the  “primary”  sensor  (i.e.,  the  joint  angle  in  our  case)  and  Rauz>  is  the 
noise  intensity  for  the  “auxiliary”  sensors  (i.e.,  the  joint  rate  and  strain  gauge  sensor). 
The  parameters  r,  are  selected  to  adjust  the  relative  “participation”  of  the  sensors  in  the 
feedback  loop.  When  the  process  noise  intensity  q  tends  to  infinity,  p  of  the  estimator 
poles  will  asymptotically  converge  towards  finite  locations  which  are  functions  of  r,.  For 
r,  =  0,  the  finite  estimator  poles  will  be  the  transmission  zeros  of  the  “primary”  sensor. 
For  r,  =  oc,  the  finite  poles  will  be  the  transmission  zeros  of  the  “auxiliary”  sensors.  The 
final  values  of  r,  are  then  chosen  so  that  the  finite  poles  are  at  some  intermediate  location 
between  these  two  sets  of  zeros.  Note  that  by  using  the  duality  between  estimator  and 
regulator  design,  these  zeros  can  be  obtained  as  the  transmission  zeros  of  a  system  of 
order  2n  similar  to  Eq.  (  3-3).  Also,  when  the  “auxiliary”  sensor  is  a  rate  sensor,  r,  is 
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Figure  3-3:  Actuator  loop  transfer  function  for  an  LQG/LTR  compensator  designed  for 
outer  elastic  link  with  the  large  payload. 

chosen  based  on  obtaining  “reasonnable”  values  for  the  DC  gains  of  the  resulting  position 
and  rate  compensators  (typically,  a  classically  designed,  low-bandwidth  PD  compensator 
is  used  as  a  baseline;  “reasonnable”  values  of  these  gains  will  correspond  to  an  increase 
by  a  factor  of  3  to  10).  Having  chosen  r„  the  process  noise  intensity  q  is  adjusted  to 
recover  the  actuator  loop  shape  given  by  the  set  of  regulator  gains. 

Using  this  method,  an  estimator  has  been  designed  for  the  large  payload  case  using 
the  joint  angle  and  velocity  sensors,  and  a  strain  gauge  sensor  located  near  the  midpoint 
of  the  outer  elastic  link.  In  each  case,  a  continuous  s-plane  design  was  done  to  select  the 
regulator  and  estimator  gains.  Using  the  resulting  continuous  compensator,  Fig.  3-3  shows 
the  actuator  loop  transfer  function.  In  this  figure,  a  Ist-order  Pade  approximation  was 
included  to  model  a  15ms  sampling  delay.  As  can  be  seen,  the  third  mode  (uncontrolled) 
is  gain  stabilized  such  that  frequency  shaping  of  the  regulator  performance  index  is  not 
required. 


3.1.2  Experimental  Results 

For  digital  implementation  on  the  experimental  testbed,  a  discrete  compensator  is  obtained 
by  applying  c  bi-linear  Tustin  transformation  to  the  continous  compensator.  For  the 
experiments  here,  a  15ms  sampling  period  was  used. 
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Figure  3-4:  Experimental  and  simulated  closed-loop  step  responses  using  an  LQG  con 
trailer  for  the  outer  elastic  link  with  the  large  payload. 


Figure  3-4  shows  the  experiment  and  simulated  step  responses  using  the  discretized 
LQG/LTR  compensator.  Excellent  agreement  is  seen  between  the  experiment  and  simu¬ 
lation.  For  performance  comparison,  a  simple  proportional  and  derivative  (PD)  controller 
has  also  been  designed;  a  first-order  low  pass  filter  is  added  in  series  with  the  PD  con¬ 
troller  in  order  to  gain  stabilize  the  unmodelled  high-frequency  modes.  Fig.  3-5  shows 
a  step  response  for  the  baseline  controller.  Comparing  the  joint  angle  traces,  the  LQG 
controller  yields  a  factor  of  two  improvement  in  rise  and  settling  times,  and  with  no 
overshoot  or  steady-state  offset.  To  compare  the  active  modal  damping  of  these  two  con¬ 
trollers  to  an  external  disturbance.  Fig.  3-6  shows  open-loop  and  closed-loop  responses 
to  a  sinusoidal  force  applied  midway  along  the  elastic  link.  Here,  the  output  from  a 
strain  gauge  is  recorded  from  the  time  the  disturbance  is  turned  off.  As  can  be  seen, 
the  LQG/LTR  controller  significantly  increases  the  closed-loop  damping  of  the  dominant 
vibration  mode  by  roughly  a  factor  of  2.5. 


3.13  Parameter  Robust  LQG/LTR  Design 

In  this  section,  we  analyze  the  closed-loop  sensitivity  of  the  LQG/LTR  controller  dis¬ 
cussed  above  to  parameter  variations  from  the  nominal  plant.  Demonstrating  the  closed- 
loop  sensitivity  to  these  changes,  we  then  discuss  a  method  that  has  been  used  to  “ro- 
bustify”  the  compensator  design.  Using  this  method,  simulation  results  demonstrate  the 
robustness  improvements  achieved. 

For  the  outer  elastic  link,  the  payload’s  moment-of-inertia  (MOI)  can  be  adjusted 
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Figure  3-5:  Experimental  and  simulated  closed-loop  step  responses  using  a  PD  controller 
for  the  outer  elastic  link  with  the  large  payload. 
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Figure  3-6:  Experimental  open-loop  and  closed-loop  responses  to  a  disturbance  force 
applied  along  the  elastic  link. 
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Figure  3-7:  Simulated  closed-loop  step  response  using  the  nominal  LQG/LTR  compensator 
for  the  outer  link  with  a  reduced  payload  moment-of  -inertia. 


whereby  the  first  resonant  (free-free)  frequency  of  the  outer  link  ranges  from  2.0 Hz  to 
4.0 Hz.  The  LQG/LTR  controller  discussed  above  was  designed  for  the  nominal  case 
of  the  maximum  payload  MOI  =  0.32fc<7  -  m2.  To  demonstrate  the  controller’s 
sensitivity  to  a  reduction  in  payload  inertia,  Fig.  3-7  shows  the  unstable  step  response 
for  a  payload  MOI  of  I'p  =  0.4/™ax.  For  this  payload  configuration,  the  first  resonant 
frequency  increases  to  3.0 Hz,  compared  to  2.0 Hz  in  the  nominal  plant.  While  this 
represents  a  significant  change  from  the  nominal  plant,  instability  actually  occurs  for 
smaller  reductions  in  lv  (on  the  order  of  0.2 /™ax). 


The  Parameter  Robust  LQG/LTR  method  is  based  on  a  numerical  re-optimization 
of  the  nominal  compensator  design  to  handle  several  open-loop  plant  models  including 
the  nominal  model  and  ones  corresponding  to  the  worst  case  combination(s)  of  modal 
parameter  variations.  Developed  by  Professor  Uy-Loi  LY,  the  software  program  SANDY 
[?]  is  used  for  doing  the  numerical  reoptimization.  The  algorithm  minimizes  a  weighted 
sum  of  n  performance  indices: 

J  =  i>,J,  (3-4) 

i=i 

where  each  index  J,  is  the  expected  value  of  the  steady- state  quadratic  performance  index 
evaluated  for  a  given  open-loop  model  obtained  for  a  given  set  of  open-loop  parameters. 
The  general  expression  for  Jx  is 


J,= 


lim  -E 
00  t  ] 


(ylQy  +  ulRu)dt 


(3-5) 


where  the  expected  value  £[•]  is  taken  over  the  ensemble  of  random  noise  disturbances 
applied  at  the  plant  input  u  (process  noise)  and  at  the  plant  outputs  y  (measurement 


Figure  3-8:  Simulated  closed-loop  step  responses  using  the  Paramter  Robust  LQG/LTR 
compensator  for  two  payload  M01  configurations:  Tip  MOl  #1  =  IrpnaT,  and  MOI  #2  = 
0.4/™“*. 

noise).  The  analog  compensator,  whose  coefficients  are  to  be  optimized  by  the  code,  is 
given  as: 


i  =  Acz  +  Bcy 

u  =  Ccz  ‘  (3-6) 

where  the  controller  matrices  (Ac,Bc,Cc)  form  a  minimal  realization  [9].  The  per¬ 
formance  index  and  its  gradients  with  respect  to  the  compensator  gains  are  evaluated 
analytically.  A  gradient  search  method  (developed  by  Gill  and  Murray)  based  on  a  modi¬ 
fied  quasi-Newton  algorithm  is  used  for  the  numerical  minimization  of  J.  The  algorithm 
will  then  find  the  elements  of  Ac,  Bc,  and  Cc,  given  an  initial  guess  (e.g.,  the  nominal 
controller).  The  resulting  compensator  will  be  stable  for  each  open-loop  model  included 
in  the  set  [1,  •  •  • ,  n]  and  is  therefore  expected  to  be  robust  to  parameter  changes. 

To  illustrate  the  method,  we  consider  the  range  of  payload  inertias  from  I™ax  to  /'. 
Using  the  performance  indices  specified  for  the  two  open-loop  models  corresponding  to 
/"“  and  /',  a  single  compensator  was  obtained.  Figure  3-8  shows  the  simulated  step 
responses  for  the  two  payload  configurations.  Despite  the  significant  plant  parameter 
variations,  the  SANDY  compensator  performs  well  for  both  cases. 
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3.2  Rigid-Flex  Control 


For  these  experiments,  the  arm  is  in  the  rigid-flex  configuration.  A  feedback  con¬ 
troller  has  been  designed  to  perform  a  large  angle  slew  maneuver  of  the  arm  in  an 
extended  configuration.  The  coupled  2-DOF  arm  dynamics  are  linearized  for  an  elbow 
angle  of  0°  (arm  extended).  The  method  of  successive  loop  closures  is  used  for  the  con¬ 
troller  design.  Using  the  same  performance  index  and  sensor  inputs  as  discussed  above, 
an  LQG/LTR  controller  is  designed  for  the  elbow  link  (with  the  shoulder  loop  open).  A 
simple  PD  controller  is  then  designed  for  the  shoulder  link  (with  the  elbow  loop  closed). 
A  second-order  lag  filter  is  added  in  series  with  the  PD  controller  in  order  to  prevent 
excitation  of  the  unmodelled  high-frequency  dynamics. 

A  fifth-order  spline  polynomial  of  time  is  used  to  generate  the  shoulder  angle  profile 
while  the  elbow  joint  angle  is  commanded  to  stay  at  0°.  To  improve  the  trajectory 
tracking,  a  feedforward  torque  command  is  also  used.  The  feedforward  torques  are 
computed  assuming  that  the  manipulator  has  rigid  links: 

%  =  Mrr(01)  °‘q  (3-7) 

where  M^Bl)  is  the  2  x  2  mass  matrix  of  the  equivalent  rigid  manipulator. 

Experimental  time  histories  of  the  joint  angles  are  shown  in  Fig.  3-9  for  2  con- 
mil  laws,  (1)  two  independent  PD  controllers  for  the  shoulder  and  elbow  actuators 
with  shaping  filters  for  gain  stabilization  of  the  high  frequency  modes  and  (2)  the  feed¬ 
back/feedforward  controller  discussed  above.  As  seen  from  the  experimental  time  histo¬ 
ries,  the  second  controller  with  the  feedforward  torque  commands  reduces  by  a  factor  of 
three  the  peak  elbow  angle  excursion  during  the  slew  maneuver  for  the  first  controller. 


3.3  Flex-Flex  Control 

In  this  section,  we  primarily  focus  on  the  design  of  a  full-state  feedback  conn-oiler 
for  the  arm  in  the  flex-flex  configuration  with  the  large  payload  [3].  In  order  to  achieve 
high-bandwidth  position  control,  the  analysis  will  show  the  importance  of  incorporating 
tip  position  outputs  into  the  regulator  performance  index.  Not  surprisingly,  the  same  will 
be  shown  true  for  the  estimator  design  as  well.  Unfortunately,  the  experimental  testbed 
has  yet  to  be  instrumented  with  a  tip  position  sensor.  As  such,  the  controller  designs 
presented  will  be  evaluated  by  simulation  using  the  nonlinear  dynamic  model  discussed 
in  the  previous  chapter. 
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Figure  3-9:  Comparison  of  two  controllers  for  a  large  angle  slew  maneuver  of  the 
rigid! elastic  planar  structure.  The  shoulder  angle  command  is  a  fifth-order  spline  function 
of  time.  The  elbow  angle  is  commanded  to  be  zero  ( extended  arm  configuration). 

3.3.1  Regulator  Designs 

A  constant  gains  linear  quadratic  regulator  (LQR)  is  designed  for  the  open-loop  system 
(A,  B,  C)  obtained  by  linearizing  the  nonlinear  dynamic  equations  (  2-2)  for  a  given  arm 
configuration.  Here,  we  have  arbitrarily  chosen  the  configuration  for  0t  =  45°.  The 
output  vector  defined  by  the  C  matrix  includes  the  joint  angles,  joint  rates,  strain  gauges 
located  on  each  link,  and  tip  position  and  velocity  coordinates. 

The  design  goal  is  to  achieve  a  closed-loop  bandwidth  for  the  rigid-body  poles  at 
least  equal  to  the  lowest  fundamental  vibration  frequency  of  the  arm  with  its  joints 
locked  (0.5  Hz  for  the  arm  configuration  chosen  here).  The  closed-loop  damping  should 
be  at  least  50%  for  the  rigid-body  poles  and  30%  for  the  dominant  elastic  modes. 

Similar  to  that  used  for  the  single  link,  the  general  form  of  the  quadratic  performance 
index  (P.I.)for  the  2-DOF  case  used  here  is: 

Jv  =  Jo  [y'Qy  +  p(v2,  +  p]'uI)}  dt  (3-8) 

where  Q  is  a  diagonal  weighting  matrix,  y  =  Cvx  is  a  user-selected  set  of  outputs,  u, 
and  it'  are  respectively  the  shoulder  and  elbow  torques.  The  relative  elbow  to  shoulder 
actuator  weighting  p,e  is  chosen  as  the  ratio  of  the  peak  shoulder  to  peak  elbow  actuator 
torques. 
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To  guide  in  selecting  the  regulated  outputs  of  y  and  the  elements  of  Q,  we  again 
make  use  of  the  optimal  regulator  root-loci  and  the  location  of  their  finite  zeros  for  a 
given  Jy.  If  the  outputs  of  y  consist  of  only  the  joint  angles  and  joint  rates,  the  finite 
zeroes  (on  the  imaginary  axis)  correspond  to  the  open-loop  vibration  frequencies  of  the 
arm  with  its  joints  locked.  This  is  illustrated  in  the  optimal  root-locus  shown  in  Fig.  3- 
lO.a.  In  this  figure,  the  poles  for  a  nominal  design  value  of  p  are  marked  by  a  shaded 
star.  The  slowest  rigid-body  poles  with  an  acceptable  damping  coefficient  will  have  a 
bandwidth  equal  to  0.5fic  at  most,  and  the  closed-loop  damping  of  the  dominant  elastic 
mode  is  about  25%. 

When  the  outputs  of  y  used  in  the  P.I.  include  only  tip  position  and  velocity  errors, 
the  design  goal  is  satisfied  for  the  rigid-body  poles  but  the  closed-loop  damping  achieved 
for  the  elastic  mode  is  less  than  10%.  As  shown  in  Fig.  3-10.b  one  pair  of  the  finite 
zeroes  is  lightly  damped  and  nearly  cancel  the  dominant  free-free  mode  of  the  arm  (2 
Hz).  This  near  pole-zero  cancellation  will  always  occur  for  the  dominant  elastic  mode  in 
the  case  of  large  tip  payload-to-arm  mass  ratio  because  in  this  vibration  mode,  the  arm 
is  nearly  pinned  at  the  tip. 

When  the  output  vector  y  includes  the  tip  position  errors,  joint  rates  and  strain 
gauge  errors,  the  corresponding  set  of  finite  zeroes  (see  Fig.  3-10.c)  will  attract  the 
dominant  closed-loop  poles  to  acceptable  locations.  For  the  nominal  value  of  p  shown,  the 
corresponding  rigid-body  bandwidth  is  now  equal  to  1 .4QC  and  the  closed-loop  damping 
for  the  dominant  elastic  modes  is  equal  to  30  %. 


Once  the  best  combination  of  sensors  to  include  in  the  performance  index  has  been 
chosen,  frequency  shaped  weightings  are  used  in  the  performance  index  to  provide  gain 
stabilization  of  the  high-frequency  unmodelled  modes.  To  simplify  the  discussion,  we 
return  to  the  case  of  the  performance  index  for  the  single  link  system  with  one  torque 
input  ue.  To  incorporate  frequency  shaped  weightings,  Eq.  3-1  becomes: 

Jj  -  f  [  ymtQ(j^)y  +  v? R{ju)ue  ]  <Lj  (3-9) 

J— oo  1  i 


where  Q(jv)  and  R[ju>)  are  frequency  dependent  weightings.  (For  our  designs,  Q(ju;) 
remains  a  diagonal  matrix  of  constant  gains.)  If  the  control  weighting  F{s  =  ju:)  is 
chosen  as  a  lag  filter: 


R(s)  =  p 


ps  +  1 

as  +  1 


(3-10) 


with  0  >  a,  the  resulting  full-state  feedback  law  will  be  of  the  form,  [6]: 


/  \  QS  +  1  T'  !  \ 

uM  =  -j7r\hAs) 


(3-11) 
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Figure  3-10:  Optimal  Regulator  Root-Loci  obtained  for  three  different  choices  of  the 
regulated  outputs.  Only  the  dominant  closed-loop  regulator  poles  obtained  as  a  function 
of  px  are  shown  here. 


frequency  (Log  Hz) 


Figure  3-11:  Magnitude  plots  of  the  shoulder  and  elbow  actuator  loop  transfer  functions 
demonstrating  the  effect  of  including  frequency  shaped  weightings  in  the  LQR  performance 
index. 

where  K  is  the  row-vector  of  state-feedback  gains  and  —jp  is  the  closed-loop  location 
for  the  lag  filter  pole.  The  resulting  actuator  loop  transfer  function  will  have  increased 
roll-off  at  high  frequency.  The  proper  choice  of  the  lag  filter  coefficients  will  result 
in  gain  stabilization  of  the  high-frequency  modes.  As  before,  parameter  p  is  used  as  a 
“tuning  knob”  to  adjust  the  rigid-body  bandwidth,  based  on  the  available  peak  torque 
from  the  actuator. 

Extending  the  method  to  the  flex-flex  case,  Fig.  3-11  demonstrates  the  frequency 
shaping  of  the  shoulder  and  elbow  actuator  loops.  Using  the  nominal  full-state  feedback 
gains  corresponding  to  Fig.  3-lO.c,  the  solid  lines  in  Fig.  3-11  are  for  the  case  without 
frequency  shaping.  Applying  frequency-shaped  weightings  results  in  the  dashed  lines  of 
Fig.  3-11  in  which  all  modes  above  10 Hz  gain  stabilized. 

In  order  to  compare  the  best  regulator  design  (i.e.,  the  one  which  regulates  tip 
position,  joint  velocity,  and  strain  gauge  errors)  to  the  simple  PD  controller  discussed  in 
the  previous  chapter  (see  section  2.3.3),  detailed  closed-loop  simulation  was  performed 
with  the  nonlinear  dynamic  model.  The  arm’s  joint  position  and  rate  commands  are 
chosen  as  a  fifth-order  spline  profile  such  that  the  tip  follows  a  straight  line  in  cartesian 
space  for  an  equivalent  rigid  manipulator.  The  initial  and  final  points  of  the  tip  trajectory 
correspond  to  6,  =  [-20°,  +20°]  and  0e  =  [+60°,  +30o]  (recall  that  the  control  design 
was  done  for  a  linearization  about  0t  =  45°).  Specified  to  reach  the  final  position  in  3 
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Figure  3-12:  Simulated  tip  reponses  for  a  constant  gain  regulator  design  and  an  indepen¬ 
dent  PD  controller. 

seconds,  the  trajectory’s  maximimum  tip  velocity  is  0.5  meters/second.  The  time  traces 
of  Fig.  3-12.(a,b)  show  the  actual  versus  commanded  tip  trajectories  obtained  for  the 
two  controllers,  while  Fig.  3-12.C  shows  these  results  plotted  in  Cartesian  space  (i.e.,  a 
“birds-eye”  view).  The  full-state  feedback  controller  maintains  good  tracking  along  the 
trajectory,  with  just  a  small  amount  of  overshoot.  Under  PD  control,  the  tip  undergoes 
large  excursions  from  the  desired  straight-line  path  and  exhibits  excessive  overshoot. 
Clearly,  the  full-state  feedback  controller — regulating  a  combination  of  tip  position  errors, 
joint  rates,  and  bending  strain — outperforms  the  colocated  PD  controller.  But  of  course, 
the  LQR  cannot  be  implemented  without  a  state  estimator,  which  is  addressed  next. 
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Figure  3-13:  Optimal  Estimator  Root-Locus  obtained  using  joint  position,  joint  velocity, 
and  strain  gauge  sensors  for  the  flex-flex  manipulator  with  a  large  payload. 

3.3.2  Estimator  Designs 

For  the  regulator  design,  the  closed-loop  rigid-body  bandwidth  requirements  were  achieved 
by  including  tip  position  as  a  regulated  output.  Following  a  similar  analysis  for  the  es¬ 
timator  design,  it  can  be  shown  that  an  end-point  sensor  is  needed  to  meet  requirements 
on  the  estimator’s  closed-loop  rigid-body  poles.  Not  having  an  end-point  position  sen¬ 
sor  available,  an  initial  estimator  has  been  designed  using  the  joint  sensors  and  strain 
gauges.  To  demonstrate  the  limitations  of  such  a  design,  we  make  use  of  the  optimal 
estimator  root-locus  (the  dual  to  the  optimal  regulator  root-locus).  Figure  3-13  shows 
the  symmetric  locus  of  closed-loop  estimator  poles  versus  the  process  noise  intensity  q. 
Similar  to  the  regulator  case,  the  circles  designate  the  finite  estimator  pole  locations,  and 
the  shaded  stars  mark  the  selected  estimator  eigenvalues  for  a  nominal  value  of  q.  While 
acceptable  damping  is  acheived  for  the  dominant  flex  mode,  the  slowest  rigid-body  pole 
(corresponding  to  the  elbow)  is  limited  to  a  bandwidth  of  0.5fic.  Nonetheless,  a  compen¬ 
sator  was  obtained  by  combining  estimator  with  the  regulator  gains  used  in  the  full-state 
feedback  controller  above. 

To  see  the  effect  of  this  less-than-desireable  estimator  design,  Figure  3-14  shows 
the  simulated  closed-loop  tip  response  using  the  same  tip  trajectory  specified  before. 
Comparing  these  plots  to  Fig.  3-12,  while  the  LQG/LTR  compensator  exhibits  slightly 
less  tracking  error  and  overshoot  than  the  PD  controller,  it  is  quite  evident  that  the 
performance  achieved  with  the  LQR  is  severely  degraded  by  the  poor  estimator  design. 
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Figure  3-14:  Simulated  tip  responses  for  the  LQGILTR  compensator. 

Once  the  experimental  manipulator  is  instrumented  with  an  end-point  position  sensor, 
future  work  will  focus  on  the  implementation  of  estimator  designs  incorporating  tip 
sensing. 

Thus  far,  we  have  limited  our  discussion  to  linear  estimators.  We  will  also  begin  to 
investigate  the  potential  benefits  of  nonlinear  estimator  designs,  such  as  a  constant-gains 
extended  Kalman  Filter  [10].  With  this  method,  instead  of  updating  the  state  estimate 
with  a  linear  transition  matrix,  the  time  update  is  obtained  by  integrating  the  nonlinear 
equations  of  motion  over  the  sample  period.  In  this  manner,  the  estimator  accounts  for 
the  nonlinear  dynamics  of  the  manipulator,  and  potentially  allows  one  set  of  regulator 
and  estimator  gains  to  achieve  high  performance  over  the  entire  workspace. 
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3.4  Conclusions 


In  this  chapter,  we  have  investigated  various  linear  LQG-based  controllers  for  three 
configurations  of  the  experimental  manipulator.  Throughout,  we  have  focused  the  con¬ 
troller  designs  for  the  manipulation  of  heavy  payloads.  For  the  single  link  case,  we  have 
demonstrated  a  method  for  improving  compensator  robustness  to  changes  in  plant  param¬ 
eters  while  maintaining  a  high  degree  of  closed-loop  performance.  The  robustification 
method  can  be  readily  extended  to  the  multi-DOF  problem.  In  the  rigid-flex  case,  the  use 
of  feed-foward  torques  based  on  the  rigid  mass  matrix  was  shown  to  improve  command 
tracking.  The  idea  can  be  applied  to  the  flex-flex  case  as  well.  Finally,  we  have  stud¬ 
ied  in  depth  the  issues  concerning  regulator  and  estimator  designs  for  the  flex-flex  case, 
showing  the  importance  of  using  a  combination  of  colocated  rate  sensors,  noncolocated 
position  sensors,  and  bending  strain  sensors. 
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Appendix  A 


Personnel  and  Interactions  with  Research 

Community 


This  appendix  gives  the  list  of  professional  personnel  and  the  interactions  with  the  re¬ 
search  community  in  the  context  of  the  current  contract. 


A.l  Personnel 


Principal  investigator: 

Dr.  Eric  Schmitz  is  a  staff  engineer  in  the  Robotics  Research  and  Technology 
Department  and  also  an  adjunct  professor  in  the  Aerospace  Engineering  Department  at  the 
University  of  Colorado  at  Boulder.  Dr.  Schmitz  obtained  his  Ph.D.degree  in  aeronautics 
and  astronautics  from  Stanford  University  in  June  1985.  The  title  of  his  doctoral  thesis  is 
“Experiments  on  the  end-point  position  control  of  a  very  flexible  one-link  manipulator”. 

Research  team: 

Madison  F.  Ramey  is  a  senior  engineer  in  the  Robotics  Research  and  Technology 
Department.  He  obtained  his  M.S.  degree  in  the  Department  of  Mechanical  Engineering 
at  Purdue  University  in  May  1986. 

Rodney  Beeston  and  Scott  Mickey  of  the  Robotics  Research  and  Technology  have 
been  instrumental  in  the  integration  of  the  experimental  testbed  (originally  designed  by 
Phil  Brunson,  Paul  Cogeos  and  Wes  Mahoney). 


A.2  Papers  Presented  at  Conferences 


The  following  papers  have  been  presented  at  conferences. 

1.  Eric  Schmitz,  “  Modeling  and  Control  of  a  Planar  Manipulator  with  an  Elastic 
Forearm  ”,  Proceedings  of  the  1989  IEEE  International  Conference  on  Robotics 


1 


and  Automation,  pp.  894-899,  Scottsdale,  Arizona 


2.  Eric  Schmitz  and  Madison  Ramey,  “  A  Simplified  Dynamic  Model  for  an  Ex¬ 
perimental  Planar  Manipulator  with  an  Elastic.  Forearm”,  presented  at  the  1989 
American  Conference,  Pittsburgh,  PA. 

3.  Madison  Ramey  and  Eric  Schmitz.  “  LQR  Design  for  an  Experimental  Planar  Elas¬ 
tic  Arm  with  a  Large  Tip  Payload”,  Proceedings  of  the  1990  American  Conference, 
pp.  1729-1732  San  Diego,  CA. 


A.3  Interactions 


We  have  been  cooperating  closely  with  Professor  A.  Von  Flotow  of  the  MIT  Aero¬ 
nautics  and  Astronautics  Department.  This  activity  has  been  formalized  with  a  subcontract 
given  to  Professor  Von  Flotow  under  the  Independent  Research  and  Development  pro¬ 
gram  of  the  Robotics  section  of  the  Research  and  Technology  Department  (D75).  Carlos 
Padilla,  a  Ph.D.  candidate  under  the  supervision  of  Professor  Von  Flotow,  has  spent  three 
months  at  Martin  Marietta  (July-August  1989,  January  1990)  working  on  the  development 
of  control  laws  for  the  elastic  arm  testbed.  Carlos  also  integrated  his  simulation  code  for 
the  flexible  arm  with  the  MATRIXx/System-Build  CAD  software.  Distributed  vibrational 
mode  sensors  using  piezoelectric  material(PVDF)  have  been  developed  by  Professor  Von 
Flotow  and  his  research  team  for  the  flexible  arm  testbed.  Testing  and  validation  of  these 
new  sensors  were  sucessfully  performed  last  January  and  are  reported  in  [12]. 

We  have  also  been  working  with  Professor  M.  Balas  of  the  Department  of  Aerospace 
Engineering  at  University  of  Colorado  at  Boulder.  The  flexible  arm  tesbed  has  been  used 
by  Professor  Balas  to  demonstrate  the  residual  mode  filter  design  technique  on  a  single 
elastic  link.  The  results  of  this  work  have  been  reported  in  [1 1].  Future  work  will  address 
the  implementation  of  an  adaptive  version  of  the  residual  mode  filter. 

Finally,  two  sessions  on  the  subject  of  dynamics  and  control  of  elastic  manipulators 
have  been  organized  by  E.  Schmitz  for  the  1989  and  1990  American  Control  Conferences. 
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Appendix  B 


Simplified  Mass  and  Stiffness  Matrices 

for  3-D  Elastic  Structure 


The  derivation  of  the  simplified  mass  and  stiffness  matrices  for  the  articulated  arm  shown  in 
Figure  B.l  is  given  in  this  section.  It  is  assumed  that  each  link  can  bend  along  two  orthogonal 
directions.  Torsional  deflections  are  neglected  here.  The  equations  of  motion  are  derived  by 
keeping  only  the  nonlinear  terms  involving  the  rigid-body  generalized  coordinates  A'r  and  A'r. 
Referring  to  equation  (2-3)  of  Section  2.2  of  the  report,  this  is  equivalent  to  computing  the 
“simplified”  mass  matrix  M(Xr)  and  stiffness  matrix  K .  The  mass  matrix  and  the  stiffness 
matrix  are  written  as: 


M 


Mrr  M re 
M  re  M  EE 


(B-l) 


K 


0  0 
0T  Kee 


(B-2) 


The  rigid-body  mass  matrix  ~M rr  can  be  readily  derived  for  the  3  DOF  rigid  structure  where 
the  two  main  links  are  assumed  to  be  rigid.  Symbolic  calculators  for  rigid  body  dynamics 
applications  such  as  [15]  can  be  used  for  that  purpose.  Therefore  the  sections  below  discusses 
only  the  derivation  of  matrices  M  re,  M ee  and  Kee- 


B.l  Kinematics 


The  inertial  frame  Ro  is  defined  as  (S,io,j0,ko)’  35  shown  in  Figure  B-l.  Frames  R},  R2 
and  R3  are  attached  to  each  body  respectively  at  each  articulation.  The  shoulder  and  elbow  links 
are  tangent  to  the  vectors  i2  and  i3  respectively  in  the  frames  R2  and  R3.  Frame  R\  is  obtained 
from  Rq  by  a  simple  rotation  ^!(k0,<?i ).  Frame  R2  is  obtained  from  Rj  by  the  simple  rotation 
R(-ive2).  Frame  Rt 2  is  attached  to  the  elbow  articulation  E  with  it2  tangent  to  the  first  link. 
Rt 2  is  related  to  R2  by  the  following  direction  cosine  matrix: 


i*2 

1  few 

h 

it2 

~ 

-few  1 

0 

^2 

.  -12  . 

.  -few  0 

l 

.  -2  . 

(B  -  3) 
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Figure  B-l:  Schematic  of  the  3-D  elastic  articulated  structure 

where  v,  and  w,  are  defined  below  as  the  two  bending  coordinates  for  the  shoulder  link  (S).  The 
direction  cosine  matrix  is  derived  with  the  assumption  that  the  elastic  deflections  and  slopes  arc 
first-order  quantities. 

Finally  frame  ft  3  is  obtained  from  Rt2  by  a  simple  rotation  ft(  kt2.03  ).  Note  that  l  is  the 
length  of  the  2  main  links. 

The  location  of  a  current  point  M,  along  the  neutral  axis  of  the  shoulder  link  (S)  is  defined 
in  R2  by  the  displacement  vector  rs: 


C9  =  xs  i2  -I-  v8(x8,t)  j2  +  wg(xs,t)  k2  (B  -  4) 

Similarly,  the  location  of  a  current  point  Me  along  the  neutral  axis  of  the  elbow  link  (E)  is  defined 
in  R 3  by  the  displacement  vector  re: 


Ce  =  xe  i3  +  ve(xe,t)j3  +  we(xe,t)  k3  ( B  —  5) 

For  both  cases,  we  have  neglected  the  second-order  axial  deformations  due  to  the  forshort- 
ening  effects.  Each  link  has  the  same  length  l. 

From  (B-4)  and  (B-5),  the  velocity  of  the  current  point  along  links  (S)  and  (E)  arc  given  as: 


vMg 


d  r2 


^2  x  r8  +  — 


_el 


(B-6) 


B-2 


(B-7) 


d  ^3 

vMe  =  VE+u>3xre  +  —  rf 

where  rg1  and  c*1  are  the  elastic  part  of  the  displacement  vectors  rs  and  re  respectively;  VE  is 
the  absolute  velocity  of  the  elbow  joint  (modelled  as  a  lumped  mass). 

The  inertial  angular  velocities  u>2  and  u>3  are  expressed  as: 


u>2 

u>2 


ko  +  ^2  ^2 
d2\s 


u>2  + 


dxdt 


(Mks 


<92ws 

dxdt 


(!•)  j2  +  83  k3 
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B.2  Linearized  Partial  Velocities 


Using  the  assumed-modes  method,  the  elastic  deflection*  o,(x.th  v.-,\x,t),  vf(x,t )  and 
ive(X't)  are  expressed  in  terms  of  the  series; 


vs(x,t)  =  ^tg(x)Qs(t);  w8(x,t)  =  $^g(x)Rs(t)  (B-9) 

V'(x,t)  =  #ve(x)Qe(t);  we(x,t  =  *we(x)Re(t)  (B- 10) 


where  Q8(t),  Qe(t),Rs(t)  and  R^t)  are  four  vectors  of  n  modal  coordinates;  <hVs(x),<I>Ws(x), 
4>ve(x)  and  3>w<?(x)  are  four  vectors  of  n  assumed  mode  shapes. 

The  velocity  Vm  for  a  point  along  the  shoulder  or  elbow  link  can  be  written  as: 

VM  =  gM  X  (B  -  11) 

where  Gm  is  a  row  vector  of  3+4n  partial  velocities.  The  kth  term  of  is  the  panial  velocity 
G^j  with  respect  to  the  kth  generalized  coordinate.  The  row  vector  Gm  can  be  expressed  as 
the  sum  of  two  vectors: 


GM  =  GM(Xr,0)  +  GM(Xr,Xei)  (B  -  12) 

In  order  to  obtain  the  simplified  dynamic  model  as  defined  in  Section  2.1,  only  the  row  vector 
Gm  need  to  be  computed. 

Accordingly,  after  pardoning  in  terms  of  rigid  and  elastic  coordinates,  the  row  vector  of 
panial  velocities  are  given  as: 


B-3 


Gm”8  =  [k,  x  x^.kj  x  x,i2,Q]  (B-13) 

G*MCl  =  [*v8j2,*ws!<2,Q.Q]  (B‘14) 

Gm"8  =  [kj  x  Ce9>  x  r«  -  *3  *  **>3]  fB'15) 

^MCl  =  fe.’g^’^eis’^weks]  <B'I6> 

with  the  foil  wing  definitions: 

I?/  =  Lh  +  X'\3  (B-17) 

g*  -  ^(Z)k2X*ei3  +  *v.(l]j2  (B- 1 8) 

e  =  _^Sa(/)j  xxeh  +  * ws(l)k2  (B-19) 

-w,  dx  -2 


B.3  Mass  Matrix  M(Xr) 

The  elements  of  the  mass  matrix  M  arc  expressed  as. 

mrs  =  t  I  Gh/G^  dm  (h  -  20) 

where  G[  is  the  rih  partial  velocity  of  a  current  point  belonging  to  body  B,.  The  submatriccs 
M he  and  M ee  arc  obtained  by  forming  scalar  products  of  partial  velocities  for  material  points 
along  the  two  elastic  links. 

B.3.1  Submatrix  Mre 

Using  equation  (B-20)  together  with  equations  (B-13)  to  (B-16),  the  3  x  4n  rigid/elastic  coupling 
mass  matrix  M  re  is  given  as: 


0  —  CjHwg  +  fi2  Q  “Zc2Pwe  ~  c(2+3)Bwe 

HVg  +  f21  Q  Z/C3P  ve  +  HVe  Q 

f3I  Q  HVe  Q 
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(B  -  21) 


where  ci  is  a  short  notation  for  cos  02'.  P„i  and  are  1  x  n  row  vectors  of  integrals  of 

the  assumed  mode  shapes  computed  along  each  elastic  link  (B): 


Pv  = 

/  $v(x)dm 

Jb 

(B-22) 

Pw  = 

/  $w(x)dm 

Jb 

(B-23) 

Hv  = 

/  i$v(x)dra 

Jb 

(B-24) 

Hw  = 

/  i$w(x)dm 

Jb 

(B-25) 

The  intermediate  row  vectors  fi2.f2i  and  f3i  are: 


<12  =  ~C3~f^§'(0  [meae<C2  +  /eC(2+3)]  ~  mf*ws(l)  [lc2  +  aoC(2+3)l  (B-26) 

<21  =  [meaelc3  +  /*]  +  mc{l  +  aec3]4>Vs(l)  (B-27) 

f31  =  meae*V8(l)  +  Ie^|^-(1)  (B-28) 

where  mr  is  the  mass  of  link  (E)  and  at  is  the  distance  from  joint  E  to  the  center  of  mass  of  the 
undeformed  link  (E). 


B.3.2  Submatrix  Mee 

From  Eq.  (B-20),  the  submatrix  M  ee  is  made  of  four  block  diagonal  submatrices  given  as: 


MvEt  = 

Js  $vsT(x)$vs(x)  dm  +  Ml 

(B-29) 

M?£“  = 

js  ^wsT(x)^ws(x)  dm  +  M2 

(B-30) 

Mve?  = 

/  #VeT(x)^ve(x)dm 

Je 

(B-31) 

MIT  = 

/  $weT(x)$we(x)dm 

(B-32) 

where  (B)  refers  to  a  given  elastic  link  and  the  two  intermediate  matrices  Mj  and  Af2  are  given 

as: 

T  T  j 

M,  =  me$V8T(l)*v8(l)+2mea€^2-  (l)*v.(l)  + 

T  T 

M2  =  m^Wg’r(l)*W8(l)  +  2mea€C3^a  (l)*Wg(l)  +  Iec§^p  (l)^^(l) 
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B.4  Stiffness  Matrix  Kee 


The  dements  of  the  stiffness  matrix  are  obtained  from  the  expression  for  the  elastic  potential 
energy: 


v-£L  H#)' 


+  Ell 


yy 


d2w, 
dx 2 


dx 


(B  -  33) 


where  EIZZ  and  EIyy  are  the  bending  stiffness  for  deflections  along  y  and  z  axes. 


From  the  series  (B-9)  and  (B-10),  the  stiffness  matrix  are  make  of  2  sets  of  block  diagonal 
sub  matrices  of  the  form: 


f  d?$vT .  , 

<P&V 

A  yy  - 

!BE'"  ds  (I) 

dx2i 

x )  dx 

fB-34) 

r  d24> wT,  . 

d2$  w 

A  WIL'  ~ 

L  EIyy  dx2  {x) 

dx2 

■( x )  dx 

(B-35) 

where  (B)  refers  to  a  given  elastic  link. 
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Appendix  C 


MATRIXx  UDF’s  and  Control  Law 

Subroutines 


In  this  appendix,  we  provide  some  of  the  MATRIXx  UDF’s  (User-Defined  Functions)  developed 
to  help  automate  the  LQG  controller  design/analysis,  and  the  C-code  listing  of  a  module  which 
implements  a  control  law  for  the  2DOF  arm. 


C.l  LQG  Design  and  Analysis  UDF’s 


The  first  two  UDF’s,  RegLocus,  and  EstLocus  are  used  to  generate,  respectivlcy,  the 
optimal  regulator  and  estimator  loci  presented  in  chapter  3.  In  generating  these  loci,  the  UDF’s 
RegTz  and  EstTz  are  used  to  compute  the  finite  zeros  (asymptotic  closed-loop  pole  locations) 
for  a  given  regulator  and  estimator  design  respectively. 


RegLocus.UDF 


// 

11 

n 

n 

n 

ii 

n 
n 
n 
// 
// 
// 
n 
n 
1 1 
n 
// 
n 
// 
n 
n 
n 
n 
// 
n 
// 
n 


(  Ev_Reg,  Tz_Reg,  Kr_Reg  I  =  RegLocus  (  S,  Ns,  Q,'  R,  Rho,  Popt  ) 

MATRIXx  function  to  generate  the  optimal  regulator  root  locus  versus  the 
control  weighting  rho.  For  each  value  of  rho  in  the  vector  Rho, 
Lqrlocus  returns  the  regulator  poles  (£v_Reg)  and  full-state  feedback 
gain  vectors  (Kr_Reg) .  As  rho  goes  to  zero,  the  regulator  poles  go  to 
the  finite  zeros  (Tz_Reg)  as  dictated  by  Q. 

Performance  index  assumed: 


J  =  Integral!  z'*diag(Q)*z  +  u'*rho*diag(R)*u  )  dt 


where:  z  *  Cx  are  the  plant  outputs  to  be  regulated 

C  =  the  output  matrix  given  in  S. 

Inputs : 


S 

Ns 

0 

R 

Rho 

Popt 


Plant  dynamics  matrix,  S  *  [  A  B  ;  C  0  ) 

Plant  order 

Row  vector  of  output  weightings 

Row  vector  of  input  weightings 

Vector  of  control  weightings,  rho 

String  of  PLOT  command  plotting  options;  if  Popt 

is  not  specified,  the  root-locus  is  not  displayed. 

(e.g.,  Popt  *  'xmin»-20  xmax*0  ymin**0  ymax*20  ...') 


Outputs: 


l 


// 

//  Ev_Reg  :=  Matrix  whose  columns  contain  the  regulator  poles 
//  for  each  value  in  Rho. 

//  Tz_Reg  :=  Vector  of  the  regulator  finite  zeros. 

//  Kr_Reg  :=  Matrix  of  Regulator  gain  vectors  for  eaoh  value  of  Rho. 

//  To  extract  the  regulator  gains  Kri  for  Rho(i),  type: 

//  Kri  =  Kr_Reg{[  Nact*i-1,  Nact*i  ],:) 

//  where  Nact  =  number  of  actuators  in  B. 


//  Author:  M.  F.  Ramey,  Martin  Marietta  Astronautics,  R&T  Group 

[  A,  B,  C,  D]  =  split (  S,  Ns  ) ; 


[Nsens,Nact)  =  size(D); 
Nq  =  max (size (Q) ) ; 

Nev  =  max (size (Rho) ) ; 


//  Do  some  simple  error  checking: 
i  f  NqONsens,  .  .  . 

di  spi. ay  (*  Error :  #  of  Output  Weightings  in  Q  .ne.  #  of  Outputs  in  C'  )  ;  .  .  . 
ret  f; .  .  . 


//  Compute  the  finite  regulator  zeros: 

7z_Reg  *  Tzreg (S, Ns, Q) ; 

If  Convert  the  output  weightings  to  state  weightings: 
Qxx  =  C'  *  diag(Q)  *  C; 

//  Form  the  input  weighting  matrix: 

RuuO  =  diag(R); 

//  Begin  the  iterations  on  Rho: 

Ruu  =  Rho(l)*RuuO;  //  First  iteration 
i  Ev_Reg,  Kr_Reg  )  =  regulator  (  A,  B,  Qxx,  Ruu  ); 

for  i=2:Nev,  ...  //  Remaining  iterations 

Ruu  =  Rho(i)*RuuO;  ... 

'  Evi,  Kri  ;  =  regulator (  A,  B,  Qxx,  Ruu  );  ... 

Ev_Reg  =  (  Ev_Reg  ,  Evi  j ;  ... 

Kr_Reg  =  (  Kr_Reg  ;  Kri  J;  ... 
end; 


//  If  Plotting  options  are  specified,  Do  plotting: 
if  exi st ( ' Popt ' ) =1 , . .  . 

plot  (real  (Ev_Reg) , imag  (Ev_Reg) , . . . 

i'xlab/Real/  ylab/ Imaginary/symbol  mark  3'  Popt));  ... 
plot ( real (Tz_Reg) , imag (Tz_Reg) ,* symbol  mark  2  keep');  ... 
end; 


//  Finis, 
ret  f 


EstLocus.UDF 

//  [  Ev_Est,  Tz_Est,  Ke_Est.  1  ■=  EstLocus  (  S,  Ns,  Kq,  Mu,  Popt  ) 

// 

//  MATRIXx  function  to  generate  the  optimal  estimator  root-locus  versus 
//  Kq,  the  process  noise  noise  intensities.  Returns  the  estimator  eigenvalues 
//  (Ev_£st)  and  gains  (Ke_Est)  for  each  value  of  Kq.  As  Kq  goes  to  infinity, 
//  the  estimator  poles  will  go  to  the  finite  estimator  zeros  (Tz_Est) 


C-2 


// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
// 
/  / 
/  / 


dictated  by  Mu. 

Inputs: 

S  :=  Continuous-time  plant  matrix  S  =  [  A  B  ;  C  0  ] 

Ns  :«  Order  of  S 

Mu  :=  Vector  of  measurement  noise  intensities  such  that 
the  measurement  noise  covariance  matrix  Qyy  is: 

Qyy  =  diag (  Mu**(-1)  );  The  elements  of  Mu  can  be 
interpreted  as  the  noise  intensity  ratios  of  primary_ser,sor  (s) 
to-auxi lliary_sensors  (s)  (mu  *  R_prim/R_aux) . 

Kq  :«  Vector  of  process  noise  intensities 

Popt  :»  Optional  string  of  PLOT  command  plotting  options. 

(e.g.,  Popt  =  'xmin*-20  xmax=0  ymin-0  ymax=2D  ...  ') 

If  Popt  is  not  specified,  the  root-locus  is  not  displayed. 

Outputs : 

Ev_Est  :=  Matrix  whose  columns  contain  the  estimator  eigen  values  for 
each  value  of  Kq. 

Tz_Est  Vector  of  finite  estimator  zeros. 

Ke_Est  :=  Matrix  of  estimator  gains  for  each  vaiue  of  Kq. 

To  extract  the  estimaior  gains  Kej  for  Kq ( j ) ,  type: 

Ke  j  =  Ke_Est<:,(  Ny*j-1,  Ny*j  !> 


//  Author: 


M.  F.  Ramey,  Martin  Marietta  Astronautics,  RS.T  Group 


A,  B,  C,  D 


split  (  S,  Ns  ) ; 


[Nsens, Nact ] =size  (D)  ; 

Nmu  =  max (size  (  Mu  j); 

Nev  =  max (size  (  Kq  ) ) ; 

//Do  some  simple  error  checking: 
i  f  NmuONsens,  .  .  . 

display  {'  Error :  ft  of  Ratios  in  Mu  <>  #  of  Sensors  in  CM;... 
ret  f ; . . . 
end; 


//  Form  the  process  noise  intensity  matrix: 

q  =  eye(Nact);  //  Equal  weightings  on  actuators’ 

QxxO  =  B*q*3' ; 

//  Form  the  measurement  noise  intensity  matrix: 

Qyy  =  diag(  Mu**(-1)  ); 

//  Compute  the  finite  estimator  'zeros': 

Tz_Est  *  Tzest (S, Ns, Mu) ; 

//  Begin  iterations  on  process  noise  intensities  Kq: 

Qxx  =  Kq(l)*OxxO;  //  First  Iteration 

(  Ev_E.it,  Ke_Est  )  *  estimator  (  A,  C,  Qxx,  Qyy  );  ... 

for  j=2:Nev,  ...  //  Remaining  Iterations 

Qxx  *  Kq(j)*QxxO;  ... 

(  Evi,  Kei  )  =  estimator (  A,  C,  Qxx,-  Qyy  );  ... 

Ev_Est  *  1  EvEst,  Evi  J;  ... 

Ke_Est  -  [  Ke_Est,  Kei  ];  ... 
end; 


//  If  Plotting  options  are  specified,  Do  plotting: 
if  exist (' Popt ') *1, .. . 

plot (real (EvEst) , imag (Ev_Est ) , . . . 
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('xlab/Real/  yl  ab/ Imaginary  /  symbol  mark  3  '  Popt));  ... 
plot (real {Tz_Est ), imag (Tz_Est) ,  '  symbol  mark  2  keep'); 
end; 

//  Finis, 
ret  f 


RegTz.UDF 

//  Tz_Reg  =  RegTz (  S,  Ns,  Q  ); 

// 

//  Returns  Tz_Reg,  the  finite  regulator  'transmission'  zeros 
//  for  the  given  plant  S  =  [  A,  B  ;  C,  0  ]  with  output  weightings  Q. 

// 

//  Performance  index  assumed: 

// 

//  J  *  Integral!  z'*diag(Q)*z  ♦  u' * rho*diag (R) *u  ]  dt 

n 

//  where:  z  =  Cx  are  the  plant  outputs  to  be  regulated 
//  C  *  the  output  matrix  given  in  S. 

/  / 

/  Inputs: 

// 

//  S  :=  Plant  dynamics  matrix,  S  =  1  A  B  ;  C  0  j 

if  Ns  :=  Pi  ant  order 

//  Q  :=  Row  vector  of  output  weightings 

/  / 

//  Outputs: 

// 

//  Tz_Reg  :=  Vector  of  the  regulator  finite  zeros. 

// 

:  A,  B,  C,  D  ]  *  split (  S,  Ns  ) ; 

(Nsen, Nact ) =size  (D) ; 

//  Convert  the  output  weightings  to  state  weightnmgs: 

Qxx  *  C'  *  diag(Q)  *  C; 

Aaug  *  [  A,  0*ones (Ns, Ns) ;  Qxx,  -A'  J; 

Baug  =  ■  B;  C*ones  (Ns, Nact )  ); 

Caug  =  [  0*cnes (Nact , Ns ) ,  -B' . ; 

//  Retain  only  the  Left-Half  Plane  Zeros: 

Tz_Reg  =  sortval (zeros ( [Aaug, Baug; Caug, 0*ones(Nact, Nact) ] , 2* Ns) ) ; 

Nz  =  max ( si ze  (Tz  Reg) ) /2; 

Tz_Reg  *  Tz_Reg (1 : Nz ) ; 

II  Finis 
ret  f 


EstTz.UDF 

//  Tz_£st  *  EstTz (  S,  Ns,  Mu  ) 

// 

//  MATRIXx  function  that  returns  a  vector  of  finite 

//  estimator  'transmission'  zeros  for  the  given  values  of  Mu. 

// 

//  S  :»  Continuous-time  plant  S  «  (  A,  B;  C  0  ]; 

//  Ns  Order  of  S 


C-4 


// 

// 

// 


Mu  :=  Vector  of  measurement  noise  intensities  such  that 
the  measurement  noise  covariance  matrix  Qyy  is: 
Qyy  =  diag(  Mu**(-1)  ) 

[  A,  B,  C,  D  ]  =  split (  S,  Ns  ) ; 

[Nsens,Nact]  =  size(  D  }; 

XX  =  C'  *  diag(Mu)  *  C  ; 

Atild  =  [  A,  0*ones<Ns,Ns)  ;  XX,  -A'  ]; 

Btild  =  [  B;  0*ones (Ns, Nact )  ]; 

Cti id  =  [  0*ones {Nact, Ns) ,  -B'  J; 

Dtiia  =  0 . 0*ones  (Nact, Nact ) ; 

Tz_Est  =  zeros ( [Atild, Btild;Ctild, Dtiid] ,  2*Ns  ); 

//  Retain  only  the  Left-Half  Plane  zeros: 

Nz  =  max (size (Tz_est) ) 12; 

Tz_Est  =  sortval  (Tz_Est ) ; 

Tz_Est  =  Tz_est (1 :Nz)  ; 

/ /  Finis, 
ret  f 


C.2  Real-Time  Controller  Module 


Here  we  list  the  C-code  for  a  generic  compensator  with  5  sensor  inputs  and  2  actuator 
output  commands.  The  structure  of  the  controller  is  as  follows: 

r(*+  1)  -  Aes(k)  +  Bce(k) 
u(*+  1)  =  Ccz{k)  +  Dce(k) 

where  (,4C,  Bc ,  Cc.  Dc)  are  the  compensator  matrices,  c  is  the  state-vector,  u  is  the  vector 
of  torque  commands,  and  e(k)  =  [yrf/(£)  —  ymca,(k)}  is  the  error  between  the  reference 
values  and  the  measured  values. 


GenComp.c 

# include  *’stdio.h> 
lin^.ude  "mfrjnult.h" 

MODULE  NAME:  GenComp 

VERSION:  1.0 

PURPOSE: 

This  module  provides  a  control  law  for  the  Flexible  Manipulator  Testbed. 
It  consists  of  a  generic  state-space  compensator  (up  to  lOth-Order)  of 
the  following  form: 

x  (k  + 1 )  «=  A*x  (k)  +  B*e  (k) 
u  (k  +  1 )  =  C*x(k)  +  D*e  (k) 
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where  (A,B,C,D)  are  the  compensator  matrices,  x  is  the  state  vector, 
u  is  the  control  vector,  and  e  is  the  error  between  the  reference 
commands  and  the  measured  signals. 

As  inputs,  the  compensator  uses  the  5  sensor  measurements  defined  for 
the  Joint  Resolvers  (2),  Joint  Tachometers  (2),  and  the  two  Strain 
channels  (2),  the  previous  sample's  actuator  command,  and  the  Shoulder  and 
Elbow  Position/Velocity  reference  commands. 

The  estimator  takes  the  error  signals  e(k)  as  input. 

As  output,  this  control  law  computes  joint  torque  commands  for  the  Shoulder 
and  Elbow  actuators  in  Newton-meters.  Any  additional  feedforward  filtering 
and  limiting  of  the  PWM  voltage  signals  is  done  by  the  calling  routine. 

Also  note  that  this  control  law  allows  the  user  to  "tweak"  loop  gains 
via  an  interactive  query  during  the  real-time  initialization. 

The  matrices  <A,B,C,D>  and  any  other  real -type  parameters  are  stored 
in  the  Real^Params  Str  structure.  The  sensor  inputs  are  stored  in  the 
Sensor  Data_Str  structure.  The  control  commands  are  storea  in  the 
Cmnd_Str  structure.  Any  integer  parameters  are  stored  in  the 
Int  Param  Str  structure. 


PROCEDURES  WITHIN  THIS  MODULE 

Ini t_Cort rol_Law  : 

Cont  rol__Law_l  : 

Controi_Law_2 

Cl ose_Cont roi^Law  : 

PROGRAMMER  NAME:  Madison  F.  Ramey 

/*  Variables  global  to  this  module 

♦include  "geniqg.h"  /* 

♦include  "flex.h" 


Sets  up  the  control  law. 

Updates  the  control  commands 

Updates  the  compensator  states  (after  I/O) 

Cleans  up  the  control  law  (usually  empty) . 


All  #aefine  ana  struct  definitions' 


/ 

/ 

/ 


PROCEDURE  NAME:  Ini t_Cont rol_Law 
PURPOSE: 

This  procedure  initializes  the  compensator's  state  and  control  vectors, 
and  queries  the  user  for  loop  tweaking  gains. 


void  Init_Cont rol_Law (Rp,  Ip) 

Real_Params_St r  *Rp; 

Int_Params_St r  *Ip; 


int  i ; 

char  ans; 

static  char  *portnamelNKTW)  =  1 

"Shldr  Loop  Gain", 

"Elbow  Loop  Gain" 

f  ; 


/*  Set  Default  Values  for  Tweaking  Gains  and  Query  user:  */ 


for  (  i*0  ;  i <NKTW  ;  ) 

K^twkli]  *  1.0; 
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printf(  "Do  you  want  to  tweak  loop  gains?  (Y/N)  ") ; 
do  i 

ans  =  get char (); 

}  while  (  (ans  !=  'Y'>  &&  (ans  !=  'N')  &&  (ans  1*  ’y')  &&  (ans  !»  'n')}; 

if  (  ans  ==  ' Y'  \\  ans  ==  *y '  )  ( 

for  (  i=0  ;  i<=2  ;  i  +  +  )  { 

printf (  "Enter  Tweak  Gain  for  %25s:  ",  portname [ i ) ) ; 
scanf (  "%f",  4K_twk(i]  ); 

) 

) 

for  (  i=0  ;  i<NKTW  ;  i+*  ) 

printf (  "%24s  Gain:  %f  \n",  portnameli],  K_twk[i]  ); 

/*  Initialize  Compensator  States:  */ 

for  (  i=0  ;  i<NX  ;  i  +  +  ) 

X_hat [i I  *  0.0; 

/*  Initialize  Compensator  Inputs:  •/ 

for  (  i*0  ;  i <NSEN  ;  i+  +  ) 

Y_err[i]  *  0.0; 

/*  Initialize  Full-State  Feedback  Terms:  */ 

for  (  i=C  ;  i <NACT  ;  i+  +  ) 

Cx [ i )  =  0.0; 

}  /*  End  of  procedure  Init _Control_Law  •/ 

PROCEDURE  NAME:  Cont rol_Law_l 
VERSION:  1.0 

PURPOSE: 

This  control  law  is  executed  immediately  after  the  sensors  are  read. 


void  Cont rol_Law_l  (Run__Time,  Reference,  Sensors, 
Rp,  Ip,  Command,  Err) 


float 

Ref _State_St  r 
Sensor_Data_St  r 
Real_Params_St  r 
Int_Params_St r 
Cmnd_St  r 

unsigned  short  int 


*Run_Time; 

•Reference; 

•Sensors; 

*Rp; 

*  ip; 

•Command; 

•Err; 


/*  16-bit  word  •/ 


float  De  [NACTJ;  /•  Direct  Transmission  Terms  •/ 

float  U_cmd  [NACTJ;  /*  Output  Torque  Command  */ 


/*  Set  the  error  signals:  */ 

/*  (note  that  Shoulder  Velocity  Ref  is  scaled  by  its  Gear  ratio  of  20)  */ 


Y_err [ 0 1 
Yerr [ 1 ] 
Y_err [2 ] 
Y_err [3] 
Y_err [4 ] 


-5ensors->Resolver (SHI 
-Sensors ->Resol ver [EL] 
-Sensors->Tachometer (SH) 
-Sensors->Tachometer [EL) 
-Sensors->Strain [SH| ; 


+  Reference->Joint_Posit ion[SH]  ; 

+  Reference-> Joint_Pos it ion [EL] ; 

♦  Reference->Joint_Velocity (SH) *20 . 0 
+  Reference->Joint_Velocity [EL) ; 
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Y_err[5]  ■*  -Sensors->strain [ELJ ; 


/*  Compute  De,  the  direct-transmission  terms: 

Multiply_NACTxNSEN_by_NSENxl  (  Rp~>D_mtx,  Y_err,  De  ); 

/*  Command  Torque  U  »  Cx  +  Du  where  Cx  is  computed  previously  in  Cont rol_Law_2 . 

U_cmd  f SH ]  =  Cx [ 0}  +  DefO]; 

U_cmd(EL)  =  Cx[l}  +  De [ 1 J ; 

/*  Transfer  to  Control  Command  Structure;  */ 

Command->Cont  rol__Command  (SH]  *  U_cmd  [SH]  *K_twk  [0]  ; 
Command->Control__Command[EL]  *  U_cmd  (EL]  *K_twk  [  1  ] ; 

•Err  *  0? 

)  /*  End  of  procedure  Cont rol__Law_l  */ 

PROCEDURE  NAME:  Cont rol_Law_2 
VERSION: 

PURPOSE: 

This  procedure  does  any  computation  after  the  torque  command  is  sent  to 
the  arm.  Everything  here  is  computed  at  the  end  of  each  frame. 

For  the  GenComp  controller,  here  1)  we  update  the  state  estimate  using 
the  linear  state-transition  matrix  and  2)  compute  the  full-state  feedback 
terms  (Cx)  needed  for  the  next  sample. 

void  Cont rol_Law_2 (Run_Time,  Reference,  Sensors, 

Rp,  Ip,  Command,  Err) 

float  *Run_Time; 

Ref_State_St r  *Reference; 

SensOL_Data_St r  *Sensors; 

Real_Params_St r  *Rp; 

Int_Params_St r  *Ip; 


Cmnd_St r 

•Command; 

unsigned  short  int 

•Err; 

/*  16-bit  word  */ 

float 

Ax 

l  NX )  ; 

/•  Acomp  •  Xhat 

*  / 

float 

Be 

I  NX !  ; 

/*  Bcomp  *  Y_err 

•/ 

int 

i 

; 

/*  Generic  Index 

*/ 

/* -  Calculate  the  next  state  estimate  :  - 

Mul t IplyNXxNXbyNXxl  (  Rp->Acomp,  X  hat.  Ax  ); 

Mul tiply_NXxNSEN_by_NSENxl  (  Rp->Bcomp,  Y_err,  Be  ); 

for  (  i*0  ;  i<NX  ;  ) 

X_hat ( i I  *  Ax [ i ]  +  Be ( i ) ; 

/*  Calculate  Full  State  Feedback  Terms  Cx  for  Next  Sample:  */ 
Multiply_NACTxNX_by_NXxl  (  Rp->Ccomp,  X_hat,  Cx  ); 

}  /*  End  of  procedure  Control_Law_2  */ 
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PROCEDURE  NAME:  Close_Cont rol^Law 
VERSION: 

PURPOSE: 

This  procedure  will  clean  up  anything  necessary  after  completion  of 
real-time  run. 

This  is  not  needed  for  the  Generic  LQG  controller. 


void  Close_Control_Law (Rp,  Ip) 

Real_Params_Str  *Rp; 

Int_Params_Str  *Ip; 


{ 

)  /*  End  of  procedure  Close_Control_Law  */ 
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